#include "getAmmo.h"
#include "CSerialPort.h"
#include "global.h"
#include "DataStruct.h"
#include <string>
#include <mutex>
#include "CuspFilter.h"

extern CSerialPort myserialport;

typedef pcl::PointXYZ PointT;
using namespace openni;
using namespace std;
mutex mu; //线程互斥对象
		pcl::PointCloud<PointT>::Ptr all_cloud_cluster(new pcl::PointCloud<PointT>);
			pcl::PointCloud<PointT>::Ptr cloud_cluster(new pcl::PointCloud<PointT>);
		CuspFilter myFilter; //初始化滤波器

SimpleOpenNIViewer::SimpleOpenNIViewer() : viewer("PCL OpenNI Viewer")
{
	normals.reset(new pcl::PointCloud<pcl::Normal>);
	cloud_filtered.reset(new pcl::PointCloud<PointT>);
	cloud_viewer.reset(new pcl::PointCloud<PointT>);
	cloud_normals.reset(new pcl::PointCloud<pcl::Normal>);
	coefficients_cylinder.reset(new pcl::ModelCoefficients);
	inliers_cylinder.reset(new pcl::PointIndices);

	sor.setLeafSize(0.003f, 0.003f, 0.003f);
	tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ>>(new pcl::search::KdTree<pcl::PointXYZ>);
	normal_estimator.setSearchMethod(tree);
	normal_estimator.setKSearch(5);
	reg.setMinClusterSize(900);
	reg.setMaxClusterSize(2500);
	reg.setSearchMethod(tree);
	reg.setNumberOfNeighbours(10);
	reg.setSmoothnessThreshold(11 / 180.0 * M_PI);
	reg.setCurvatureThreshold(0.5);

} // Construct a cloud viewer, with a window name

// ����ص�����cloud_cb_,��ȡ������ʱ�����ݽ��д���
void SimpleOpenNIViewer::cloud_cb_(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud)
{
	if (!viewer.wasStopped())
	{
		int times = 0;
		short pre_data[10][4]; //0 x;1 z;2 size;  int *p
		DataStruct true_data = {0, 0, 0, 0, 0x5A};
		DataStruct filt_data = {0, 0, 0, 0, 0x5A};
		DataStruct send_data = {0, 0, 0, 0, 0x5A};
		short x, angle, z = 0.5;
		// Build a passthrough filter to remove spurious NaNs

		pass.setInputCloud(cloud);
		pass.setFilterFieldName("y");
		pass.setFilterLimits(y_min_getammo, y_max_getammo); //-0.04, 0.06    -0.08, 0.01   table0.04, 0.14
		pass.filter(*cloud_viewer);

		sor.setInputCloud(cloud_viewer);
		sor.filter(*cloud_filtered);

		//std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl;
		tree->setInputCloud(cloud_filtered);
		std::vector<pcl::PointIndices> cluster_indices;
		ec.setClusterTolerance(0.015); // 1.3cm
		ec.setMinClusterSize(800);
		ec.setMaxClusterSize(3000);
		ec.setSearchMethod(tree);
		ec.setInputCloud(cloud_filtered);
		ec.extract(cluster_indices);

		all_cloud_cluster->clear();
		all_cloud_cluster->resize(cloud_filtered->size());
		for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
		{
			//cout<<"size:"<<cloud_cluster->size()<<"\t";
cloud_cluster->clear();
			//cout<<cloud_cluster->size()<<"\n";

			for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
				cloud_cluster->points.push_back(cloud_filtered->points[*pit]);
			cloud_cluster->width = cloud_cluster->points.size();
			cloud_cluster->height = 1;
			cloud_cluster->is_dense = true;
			//cout << cloud_cluster->size() << endl;
			viewer.showCloud(cloud_cluster);

			if (cloud_cluster->size() > 0)
			{
				ne.setSearchMethod(tree);
				ne.setInputCloud(cloud_cluster);
				ne.setKSearch(50);
				ne.compute(*cloud_normals);

				seg.setOptimizeCoefficients(true);
				seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
				seg.setNormalDistanceWeight(0.1);
				seg.setMethodType(pcl::SAC_RANSAC);
				seg.setMaxIterations(100);
				seg.setDistanceThreshold(0.005);
				seg.setInputCloud(cloud_cluster);
				seg.setInputNormals(cloud_normals);
				pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients);
				pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices);
				// Obtain the plane inliers and coefficients
				seg.segment(*inliers_plane, *coefficients_plane);
				float a, b, c, d;
				float distance = 0, angle = 0;
				Eigen::Vector4f centroid; //质心
				if (coefficients_plane->values.size() > 0)
				{
					a = coefficients_plane->values[0];
					b = coefficients_plane->values[1];
					c = coefficients_plane->values[2];
					d = coefficients_plane->values[3];
					angle = (atan2(c, a) * 180 / 3.14 - 90) * 100;
					distance = (abs(d) / sqrt(a * a + b * b + c * c)) * 1000;
					if (c != 0)
						//cout << "角度:                       " << angle << endl;
					//cout << "距离:                             " << distance << endl;

tt_test.tic();
					pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extractor;
					feature_extractor.setInputCloud(cloud_cluster);
					feature_extractor.compute();
					pcl::PointXYZ min_point_AABB;
					pcl::PointXYZ max_point_AABB;
					feature_extractor.getAABB(min_point_AABB, max_point_AABB);
					x = ((max_point_AABB.x + min_point_AABB.x) / 2) * 10000;
//cout<<"feat Done:"<<int(tt_test.toc())<<"\t";
tt_test.tic();

					pcl::compute3DCentroid(*cloud_cluster, centroid); //估计质心的坐标
//cout<<"cent Done:"<<int(tt_test.toc())<<"\n";
					//cout << "左右(x cent):                                   " << x << "," << centroid[0] * 10000 << endl;
				}

				all_cloud_cluster->operator+=(*cloud_cluster);
				pre_data[times][0] = x;
				pre_data[times][1] = centroid[0] * 10000+50;
				pre_data[times][2] = angle;
				//cout << "all(size,x,weight,z,angle):" << cloud_cluster->size() << "   " << pre_data[times][1] << "  " << max_point_AABB.z - min_point_AABB.z << "  " << pre_data[times][2] << "  " << pre_data[times][3] << "  " << atan2((max_point_AABB.z - min_point_AABB.z), (max_point_AABB.x - min_point_AABB.x)) << endl;
				times++;
			}
		}

		if (times == 1)
		{
			x = -pre_data[0][0];
			z = pre_data[0][1];
			angle = -pre_data[0][2];
			send_data.Flag = 2;
		}
		else if (times > 1)
		{
			send_data.Flag = 2;
			short temp_x, max_n;
			max_n = 0;
			temp_x = 30000;
			for (int i = 0; i < times; i++)
			{
				if (abs(pre_data[i][0]) < temp_x)
				{
					x = -pre_data[i][0];
					z = pre_data[i][1];
					angle = -pre_data[i][2];

					temp_x = abs(pre_data[i][0]);
				}
			}
		}
		else
		{
			x = z = angle = 0;
			send_data.Flag = 0;
		}

		short last_x = 0;
		send_data.X = x;
		send_data.Y = z;
		send_data.Z = angle;
		send_data.end = 0x5A;
		//std::cout << "x:" << send_data.X << "  z:" << send_data.Y << "  angle:" << send_data.Z << "\n\n\n";
		 mu.lock();
		myFilter.filter(send_data, filt_data);
		 mu.unlock();
		//myserialport.writeToCom_char((char *)&filt_data, sizeof(filt_data));
		//std::cerr << "  end  Done: " << int(tt.toc()) << " ms\n\n";
		//system("pause");
		//if (all_cloud_cluster->size() > 0)
		//viewer.showCloud(cloud_viewer);
		tt.tic();
	}
}

void SimpleOpenNIViewer::run()
{
	OpenNI::initialize();
	Array<DeviceInfo> aDeviceList;

	while (true)
	{
		OpenNI::enumerateDevices(&aDeviceList);
		while (aDeviceList.getSize() == 0)
		{
			OpenNI::enumerateDevices(&aDeviceList);
			usleep(1);
		}
		// create a new grabber for OpenNI devices
		pcl::Grabber *interface = new pcl::io::OpenNI2Grabber();

		// make callback function from member function
		boost::function<void(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &)> f =
			boost::bind(&SimpleOpenNIViewer::cloud_cb_, this, _1);

		// connect callback function for desired signal
		boost::signals2::connection c = interface->registerCallback(f);

		// start receiving point clouds
		interface->start();

		while (!viewer.wasStopped())
		{
			OpenNI::enumerateDevices(&aDeviceList);
			if (aDeviceList.getSize() == 0 || mode != 0x02)
				break;
			boost::this_thread::sleep(boost::posix_time::seconds(0.001));
		}
		interface->stop();
		delete interface;
		cout << "getammo break" << endl;
		if (mode != 0x02)
			break;
	}
}
